home *** CD-ROM | disk | FTP | other *** search
/ 3D GFX / 3D GFX.iso / amiutils / i_l / irit5 / cagd_lib / cbsp_int.c < prev    next >
C/C++ Source or Header  |  1995-12-30  |  13KB  |  324 lines
  1. /******************************************************************************
  2. * CBsp-Int.c - Bspline curve interpolation/approximation schemes.          *
  3. *******************************************************************************
  4. * Written by Gershon Elber, Feb. 94.                          *
  5. ******************************************************************************/
  6.  
  7. #include <ctype.h>
  8. #include <stdio.h>
  9. #include <string.h>
  10. #include "cagd_loc.h"
  11. #include "extra_fn.h"
  12.  
  13. /*****************************************************************************
  14. * DESCRIPTION:                                                               M
  15. * Given a set of points, PtList, computes a Bspline curve of order Order     M
  16. * that interpolates or least square approximates the set of points.         M
  17. *   The size of the control polygon of the resulting Bspline curve defaults  M
  18. * to the number of points in PtList (if CrvSize = 0).                 M
  19. *   However, this number is can smaller to yield a least square              M
  20. * approximation.                                 M
  21. *   The created curve can be parametrized as specified by ParamType.         M
  22. *                                                                            M
  23. *                                                                            *
  24. * PARAMETERS:                                                                M
  25. *   PtList:       List of points to interpolate/least square approximate.    M
  26. *   Order:        Of interpolating/approximating curve.                      M
  27. *   CrvSize:      Number of degrees of freedom (control points) of the       M
  28. *                 interpolating/approximating curve.                         M
  29. *   ParamType:    Type of parametrization.                                   M
  30. *                                                                            *
  31. * RETURN VALUE:                                                              M
  32. *   CagdCrvStruct *:   Constructed interpolating/approximating curve.        M
  33. *                                                                            *
  34. * KEYWORDS:                                                                  M
  35. *   BspCrvInterpPts, interpolation, least square approximation               M
  36. *****************************************************************************/
  37. CagdCrvStruct *BspCrvInterpPts(CagdPtStruct *PtList,
  38.                    int Order,
  39.                    int CrvSize,
  40.                    CagdParametrizationType ParamType)
  41. {
  42.     int i, NumPts;
  43.     CagdPtStruct *Pt;
  44.     CagdRType *KV, *PtKnots, *AveKV, *R, *R2, t;
  45.     CagdCrvStruct *Crv;
  46.     CagdCtlPtStruct
  47.     *CtlPt = NULL,
  48.     *CtlPtList = NULL;
  49.  
  50.     for (NumPts = 0, Pt = PtList; Pt != NULL; NumPts++, Pt = Pt -> Pnext);
  51.     if (CrvSize == 0)
  52.     CrvSize = NumPts;
  53.     if (NumPts < 3 || NumPts < Order || NumPts < CrvSize || CrvSize < Order)
  54.     return NULL;
  55.  
  56.     R = PtKnots = (CagdRType *) IritMalloc(sizeof(CagdRType) * NumPts);
  57.  
  58.     /* Compute parameter values at which the curve will interpolate PtList. */
  59.     switch (ParamType) {
  60.     case CAGD_CHORD_LEN_PARAM:
  61.         *R++ = 0.0;
  62.         for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++)
  63.         *R = *(R - 1) + PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt);
  64.         for (t = R[-1]; R != PtKnots; *--R /= t);
  65.         break;
  66.     case CAGD_CENTRIPETAL_PARAM:
  67.         *R++ = 0.0;
  68.         for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++)
  69.         *R = *(R - 1) + sqrt(PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt));
  70.         for (t = R[-1]; R != PtKnots; *--R /= t);
  71.         break;
  72.     case CAGD_UNIFORM_PARAM:
  73.     default:
  74.         for (i = 0; i < NumPts; i++)
  75.         *R++ = ((RealType) i) / (NumPts - 1);
  76.         break;
  77.     }
  78.  
  79.     /* Construct the knot vector of the Bspline curve. */
  80.     KV = (CagdRType *) IritMalloc(sizeof(CagdRType) * (CrvSize + Order));
  81.  
  82.     AveKV = BspKnotAverage(PtKnots, NumPts, NumPts + Order - CrvSize - 1);
  83.     BspKnotAffineTrans(AveKV, CrvSize - Order + 2, PtKnots[0] - AveKV[0],
  84.                (PtKnots[NumPts - 1] - PtKnots[0]) /
  85.                (AveKV[CrvSize - Order + 1] - AveKV[0]));
  86.  
  87.     for (i = 0, R = KV, R2 = AveKV; i < Order; i++)
  88.     *R++ = *R2;
  89.     for (i = 0; i < CrvSize - Order; i++)
  90.     *R++ = *++R2;
  91.     for (i = 0, R2++; i < Order; i++)
  92.     *R++ = *R2;
  93.  
  94.     IritFree((VoidPtr) AveKV);
  95.  
  96.     /* Convert to control points in a linear list */
  97.     for (Pt = PtList; Pt != NULL; Pt = Pt -> Pnext) {
  98.     if (CtlPtList == NULL)
  99.         CtlPtList = CtlPt = CagdCtlPtNew(CAGD_PT_E3_TYPE);
  100.     else {
  101.         CtlPt ->Pnext = CagdCtlPtNew(CAGD_PT_E3_TYPE);
  102.         CtlPt = CtlPt -> Pnext;
  103.     }
  104.     for (i = 0; i < 3; i++)
  105.         CtlPt -> Coords[i + 1] = Pt -> Pt[i];
  106.     }
  107.     Crv = BspCrvInterpolate(CtlPtList, NumPts, PtKnots, KV, CrvSize, Order,
  108.                 FALSE);
  109.     CagdCtlPtFreeList(CtlPtList);
  110.  
  111.     IritFree((VoidPtr *) PtKnots);
  112.     IritFree((VoidPtr *) KV);
  113.  
  114.     return Crv;
  115. }
  116.  
  117. /*****************************************************************************
  118. * DESCRIPTION:                                                               M
  119. * Given set of points, PtList, parameter values the curve should interpolate M
  120. * or approximate these points, Params, and the expected knot vector, KV,     M
  121. * length Length and order Order of the Bspline curve, computes the Bspline   M
  122. * curve's coefficients.                                 M
  123. *   All points in PtList are assumed of the same type.                 M
  124. *   If Periodic, Order - 1 more constraints (and DOF's) are added so that    M
  125. * the first Order - 1 points are the same as the last Order - 1 points.         M
  126. *                                                                            *
  127. * PARAMETERS:                                                                M
  128. *   PtList:       List of points to interpolate/least square approximate.    M
  129. *   NumPts:       Size of PtList.
  130. *   Params:       At which to interpolate the points in PtList.              M
  131. *   KV:           Computed knot vector for the constructed curve.            M
  132. *   Length:       Number of degrees of freedom (control points) of the       M
  133. *                 interpolating/approximating curve.                         M
  134. *   Order:        Of interpolating/approximating curve.                      M
  135. *   Periodic:     If dat a and hence constructed curve should be Periodic.   M
  136. *                                                                            *
  137. * RETURN VALUE:                                                              M
  138. *   CagdCrvStruct *:   Constructed interpolating/approximating curve.        M
  139. *                                                                            *
  140. * KEYWORDS:                                                                  M
  141. *   BspCrvInterpolate, interpolation, least square approximation             M
  142. *****************************************************************************/
  143. CagdCrvStruct *BspCrvInterpolate(CagdCtlPtStruct *PtList,
  144.                  int NumPts,
  145.                  CagdRType *Params,
  146.                  CagdRType *KV,
  147.                  int Length,
  148.                  int Order,
  149.                  CagdBType Periodic)
  150. {
  151.     int i, j, k,
  152.     PerNumPts = NumPts + (Periodic ? Order - 1 : 0);
  153.     CagdCtlPtStruct *Pt;
  154.     CagdRType *Mat, *InterpPts, *M, *R;
  155.     CagdCrvStruct *Crv;
  156.     CagdPointType
  157.     PtType = PtList -> PtType;
  158.  
  159.     /* Construct the Bspline curve and its knot vector. */
  160.     Crv = BspPeriodicCrvNew(Length, Order, Periodic, PtType);
  161.     CAGD_GEN_COPY(Crv -> KnotVector, KV,
  162.           (CAGD_CRV_PT_LST_LEN(Crv) + Order) * sizeof(CagdRType));
  163.  
  164.     /* Construct the system of equations' matrix. */
  165.     M = Mat = (CagdRType *) IritMalloc(sizeof(CagdRType) * Length * PerNumPts);
  166.     for (i = 0, R = Params; i < PerNumPts; i++, R++, M += Length) {
  167.     int Index;
  168.     CagdRType
  169.         *Line = BspCrvCoxDeBoorBasis(KV, Order,
  170.                      Length + (Periodic ? Order - 1 : 0),
  171.                      *R, &Index);
  172.  
  173.     for (k = 0; k < Length; k++)
  174.         M[k] = 0.0;
  175.     for (j = Order, k = Index; j > 0; j--, k++)
  176.         M[k % Length] = *Line++;
  177.     }
  178.  
  179.     /* Compute SVD decompostion for Mat. */
  180.     SvdLeastSqr(Mat, NULL, NULL, PerNumPts, Length);
  181.     IritFree((VoidPtr *) Mat);
  182.  
  183.     /* Solve for the coefficients of all the coordinates of the curve. */
  184.     InterpPts = (CagdRType *) IritMalloc(sizeof(CagdRType) * PerNumPts);
  185.     for (i = !CAGD_IS_RATIONAL_PT(PtType);
  186.      i <= CAGD_NUM_OF_PT_COORD(PtType);
  187.      i++) {
  188.     for (Pt = PtList, R = InterpPts, j = PerNumPts;
  189.          j > 0;
  190.          Pt = Pt -> Pnext, j--)
  191.         *R++ = Pt -> Coords[i];
  192.  
  193.     SvdLeastSqr(NULL, Crv -> Points[i], InterpPts, NumPts, Length);
  194.     }
  195.     IritFree((VoidPtr *) InterpPts);
  196.  
  197.     return Crv;
  198. }
  199.  
  200.  
  201. /*****************************************************************************
  202. * DESCRIPTION:                                                               M
  203. * Given a set of points, and a curve least square fitting them using the     M
  204. * BspCrvInterpPts function, computes an error measure as a the maximal         M
  205. * distance between the curve and points (L1 norm).                 M
  206. *                                                                            *
  207. * PARAMETERS:                                                                M
  208. *   Crv:        Curve that was fitted to the data set.                       M
  209. *   PtList:     The data set.                                                M
  210. *   ParamType:  Parameter values at with curve should interpolate PtList.    M
  211. *                                                                            *
  212. * RETURN VALUE:                                                              M
  213. *   CagdRType:   Error measured in the L1 norm.                              M
  214. *                                                                            *
  215. * KEYWORDS:                                                                  M
  216. *   BspCrvInterpPtsError, error estimation, interpolation, least square      M
  217. *   approximation                                                            M
  218. *****************************************************************************/
  219. CagdRType BspCrvInterpPtsError(CagdCrvStruct *Crv,
  220.                    CagdPtStruct *PtList,
  221.                    CagdParametrizationType ParamType)
  222. {
  223.     int i, NumPts;
  224.     CagdPtStruct *Pt;
  225.     CagdRType *PtKnots, *R, t,
  226.     MaxDist = 0.0;
  227.  
  228.     for (NumPts = 0, Pt = PtList; Pt != NULL; NumPts++, Pt = Pt -> Pnext);
  229.  
  230.     R = PtKnots = (CagdRType *) IritMalloc(sizeof(CagdRType) * NumPts);
  231.  
  232.     /* Compute parameter values at which the curve will interpolate PtList. */
  233.     switch (ParamType) {
  234.     case CAGD_CHORD_LEN_PARAM:
  235.         *R++ = 0.0;
  236.         for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++)
  237.         *R = *(R - 1) + PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt);
  238.         for (t = R[-1]; R != PtKnots; *--R /= t);
  239.         break;
  240.     case CAGD_CENTRIPETAL_PARAM:
  241.         *R++ = 0.0;
  242.         for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++)
  243.         *R = *(R - 1) + sqrt(PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt));
  244.         for (t = R[-1]; R != PtKnots; *--R /= t);
  245.         break;
  246.     case CAGD_UNIFORM_PARAM:
  247.     default:
  248.         for (i = 0; i < NumPts; i++)
  249.         *R++ = ((RealType) i) / (NumPts - 1);
  250.         break;
  251.     }
  252.  
  253.     for (i = 0, Pt = PtList; i < NumPts; i++, Pt = Pt -> Pnext) {
  254.     CagdRType Dist;
  255.  
  256.     R = CagdCrvEval(Crv, PtKnots[i]);
  257.     R = &R[1];
  258.  
  259.     Dist = PT_PT_DIST(R, Pt->Pt);
  260.     if (Dist > MaxDist)
  261.         MaxDist = Dist;
  262.     }
  263.  
  264.     return MaxDist;
  265. }
  266.  
  267. /*****************************************************************************
  268. * DESCRIPTION:                                                               M
  269. *   Computes zero and first moments of a curve.                              M
  270. *                                                                            *
  271. * PARAMETERS:                                                                M
  272. *   Crv:     To compute zero and first moment.                               M
  273. *   n:       Number of samples the curve should be sampled at.               M
  274. *   Loc:     Center of curve as zero moment.                                 M
  275. *   Dir:     Main direction of curve as first moment.                        M
  276. *                                                                            *
  277. * RETURN VALUE:                                                              M
  278. *   void                                                                     M
  279. *                                                                            *
  280. * KEYWORDS:                                                                  M
  281. *   CagdCrvFirstMoments                                                      M
  282. *****************************************************************************/
  283. void CagdCrvFirstMoments(CagdCrvStruct *Crv,
  284.              int n,
  285.              CagdPType Loc,
  286.              CagdVType Dir)
  287. {
  288.     int i;
  289.     CagdRType t, TMin, TMax, Dt, **Points;
  290.     CagdPtStruct *Pt,
  291.     *PtList = NULL;
  292.     CagdCrvStruct *InterpCrv;
  293.  
  294.     if (n < 2)
  295.     n = 2;
  296.  
  297.     CagdCrvDomain(Crv, &TMin, &TMax);
  298.     t = TMin;
  299.     Dt = (TMax - TMin) / (n - 1);
  300.     for (i = 0; i < n; i++, t += Dt) {
  301.     RealType
  302.         *R = CagdCrvEval(Crv, t);
  303.  
  304.     Pt = CagdPtNew();
  305.     CagdCoerceToE3(Pt -> Pt, &R, -1, Crv -> PType);
  306.     LIST_PUSH(Pt, PtList);
  307.     }
  308.  
  309.     InterpCrv = BspCrvInterpPts(PtList, 2, 2, CAGD_UNIFORM_PARAM);
  310.     CagdPtFreeList(PtList);
  311.     Points = InterpCrv -> Points;
  312.  
  313.     Loc[0] = (Points[1][0] + Points[1][1]) / 2.0;
  314.     Loc[1] = (Points[2][0] + Points[2][1]) / 2.0;
  315.     Loc[2] = (Points[3][0] + Points[3][1]) / 2.0;
  316.  
  317.     Dir[0] = (Points[1][1] - Points[1][0]);
  318.     Dir[1] = (Points[2][1] - Points[2][0]);
  319.     Dir[2] = (Points[3][1] - Points[3][0]);
  320.     PT_NORMALIZE(Dir);
  321.  
  322.     CagdCrvFree(InterpCrv);
  323. }
  324.